#include "stm32f1xx_hal.h"
#include "rtthread.h"
#include "drv_mpu6050.h"
#include "stm32f1xx_hal_gpio.h"

#define SCL GPIO_PIN_6
#define SDA GPIO_PIN_7
//直接修改寄存器来配置，比使用hal库初始化更方便
//#define SDA_IN() {GPIOB->CRH&=0XFFFFFF0F;GPIOB->CRH|=(uint32_t)8<<4;}
//#define SDA_OUT() {GPIOB->CRH&=0XFFFFFF0F;GPIOB->CRH|=(uint32_t)1<<4;}

void SCL_HIGH() {HAL_GPIO_WritePin(GPIOB,SCL,GPIO_PIN_SET);}
void SCL_LOW() {HAL_GPIO_WritePin(GPIOB,SCL,GPIO_PIN_RESET);}
void SDA_HIGH() {HAL_GPIO_WritePin(GPIOB,SDA,GPIO_PIN_SET);}
void SDA_LOW() {HAL_GPIO_WritePin(GPIOB,SDA,GPIO_PIN_RESET);}


void SDA_IN(void){
    GPIO_InitTypeDef GPIO_InitStruct={0};
    GPIO_InitStruct.Pin = SDA;
    GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
    GPIO_InitStruct.Pull = GPIO_PULLUP;
    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
    HAL_GPIO_Init(GPIOB,&GPIO_InitStruct);
}

void SDA_OUT(void){
    GPIO_InitTypeDef GPIO_InitStruct;
    GPIO_InitStruct.Pin = SDA;
    GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
    GPIO_InitStruct.Pull = GPIO_NOPULL;
    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
    HAL_GPIO_Init(GPIOB,&GPIO_InitStruct);
}


//IIC初始化
void IIC_Init(){
    SCL_HIGH();
    SDA_HIGH();
}
//IIC起始信号
void IIC_Start(void) {
    SDA_OUT();
    SDA_HIGH();
    SCL_HIGH();
    rt_hw_us_delay(4);
    SDA_LOW();
    rt_hw_us_delay(4);
    SCL_LOW();
}
//停止信号
void IIC_Stop(){
    SDA_OUT();
    SCL_LOW();
    SDA_LOW();
    rt_hw_us_delay(4);
    SCL_HIGH();
    rt_hw_us_delay(4);
    SDA_HIGH();
}
//等待应答信号
uint8_t IIC_Wait_Ack(){
    uint8_t ErrTim = 0;
    SDA_IN();
    SDA_HIGH();
    rt_hw_us_delay(1);
    SCL_HIGH();
    rt_hw_us_delay(1);
    while(HAL_GPIO_ReadPin(GPIOB,GPIO_PIN_9)){
        ErrTim++;
        if(ErrTim>250){
            IIC_Stop();
            return 1;
        }
    }
    SCL_LOW();
    return 0;
}
//应答信号
void IIC_Ask(){
    SCL_LOW();
    SDA_OUT();
    SDA_LOW();
    rt_hw_us_delay(2);
    SCL_HIGH();
    rt_hw_us_delay(2);
    SCL_LOW();
}
//非应答信号
void IIC_NAck(){
    SCL_LOW();
    SDA_OUT();
    SDA_HIGH();
    rt_hw_us_delay(2);
    SCL_HIGH();
    rt_hw_us_delay(2);
    SCL_LOW();
}
//IIC发送一个字节
void IIC_Send_Byte(uint8_t txMes){
    uint8_t t;
    SDA_OUT();
    SCL_LOW();
    for (t = 0; t < 8; t++) {
        if((txMes & 0x80)>>7){
            SDA_HIGH();
        }else{
            SDA_LOW();
        }
        txMes<<=1;
        rt_hw_us_delay(2);
        SCL_HIGH();
        rt_hw_us_delay(2);
        SCL_LOW();
        rt_hw_us_delay(2);
    }
}
//IIC读取一个字节
uint8_t  IIC_Read_Byte(){
    unsigned char i,receive = 0;
    SDA_IN();
    for(i=0;i<8;i++){
        receive<<=1;
        SCL_LOW();
        rt_hw_us_delay(4);
        SCL_HIGH();
        rt_hw_us_delay(4);
        if(HAL_GPIO_ReadPin(GPIOB,GPIO_PIN_9)){
            receive |= 0x01;
        }
        rt_hw_us_delay(4);
    }
    SCL_LOW();
    return receive;
}
//IIC往指定从设备寄存器写入一个字节数据
/*
 * @SlaveAddr   从设备地址，包含读写标志位
 * @REG_Addr    从设备的寄存器地址
 * @REG_data    发送的数据
 * */

uint8_t IIC_Write_1Byte(uint8_t SlaveAddr,uint8_t REG_Addr,uint8_t REG_data){
    IIC_Start();
    IIC_Send_Byte(SlaveAddr);
    if(IIC_Wait_Ack()){
        IIC_Stop();
        return 1;
    }
    IIC_Send_Byte(REG_Addr);
    IIC_Wait_Ack();
    //rt_hw_us_delay(5);
    IIC_Send_Byte(REG_data);
    if(IIC_Wait_Ack()){
        IIC_Stop();
        return 1;
    }
    IIC_Stop();

    return 0;
}
//从从设备指定寄存器读取一个字节
uint8_t IIC_Read_1Byte(uint8_t SlaveAddr,uint8_t REG_Addr,uint8_t * REG_data){
    IIC_Start();
    IIC_Send_Byte(SlaveAddr);
    if(IIC_Wait_Ack()){
        IIC_Stop();
        return 1;
    }
    IIC_Send_Byte(REG_Addr);
    IIC_Wait_Ack();
   // rt_hw_us_delay(5);
    IIC_Start();
    IIC_Send_Byte(SlaveAddr|0x01);
    IIC_Wait_Ack();
    *REG_data = IIC_Read_Byte();
    IIC_Stop();

    return 0;
}
//读取多个字节数据
uint8_t IIC_Read_nByte(uint8_t SlaveAddr,uint8_t REG_Addr,uint8_t * REG_data,uint16_t len){
    IIC_Start();
    IIC_Send_Byte(SlaveAddr);
    if(IIC_Wait_Ack()){
        IIC_Stop();
        return 1;
    }
    IIC_Send_Byte(REG_Addr);
    IIC_Wait_Ack();
    rt_hw_us_delay(5);
    IIC_Start();
    IIC_Send_Byte(SlaveAddr|0x01);
    IIC_Wait_Ack();
    while (len){
        *REG_data = IIC_Read_Byte();
        if(1 == len){
            IIC_NAck();
        }else{
            IIC_Ask();
        }
        REG_data++;
        len--;
    }
    IIC_Stop();

    return RT_EOK;
}
//写入多个字节数据
uint8_t IIC_Write_nByte(uint8_t SlaveAddr,uint8_t REG_Addr,uint8_t * REG_data,uint16_t len){
    uint8_t i;
    IIC_Start();
    IIC_Send_Byte(SlaveAddr);
    if(IIC_Wait_Ack()){
        IIC_Stop();
        return 1;
    }
    IIC_Send_Byte(REG_Addr);
    IIC_Wait_Ack();
   for(i=0;i<len;i++){
        IIC_Send_Byte(REG_data[i]);
        if(IIC_Wait_Ack()){
            IIC_Stop();
            return 1;
        }
    }
    IIC_Stop();

    return RT_EOK;
}













/*********************************************************************************************************
* 函 数 名 : Mpu6050_WriteLen
* 功能说明 : IIC连续写
* 形    参 : addr：器件地址；reg：寄存器地址；len：写入长度；buf：数据区；
* 返 回 值 : 0：成功；其他：失败
* 备    注 : 无
*********************************************************************************************************/
unsigned char Mpu6050_WriteLen(unsigned char addr, unsigned char reg, unsigned char len, unsigned char *buf)
{
    unsigned char i;

    IIC_Start();
    IIC_WriteByte((addr<<1) | 0);   //发送器件地址+写命令
    if(IIC_Wait_ACK()){             //等待应答
        IIC_Stop();
        return 1;
    }

    IIC_WriteByte(reg);             //写寄存器地址
    IIC_Wait_ACK();                 //等待应答
    for(i=0; i<len; i++){
        IIC_WriteByte(buf[i]);      //发送数据
        if(IIC_Wait_ACK()){         //等待ACK
            IIC_Stop();
            return 1;
        }
    }
    IIC_Stop();

    return 0;
}
/*********************************************************************************************************
* 函 数 名 : Mpu6050_ReadLen
* 功能说明 : IIC连续读
* 形    参 : addr：器件地址；reg：寄存器地址；len：写入长度；buf：数据区；
* 返 回 值 : 0：成功；其他：失败
* 备    注 : 无
*********************************************************************************************************/
unsigned char Mpu6050_ReadLen(unsigned char addr, unsigned char reg, unsigned char len, unsigned char *buf)
{
    IIC_Start();
    IIC_WriteByte((addr<<1) | 0);   //发送器件地址+写命令
    if(IIC_Wait_ACK()){             //等待应答
        IIC_Stop();
        return 1;
    }

    IIC_WriteByte(reg);             //写寄存器地址
    IIC_Wait_ACK();                 //等待应答
    IIC_Start();
    IIC_WriteByte((addr<<1) | 1);   //发送器件地址+读命令
    IIC_Wait_ACK();                 //等待应答
    while(len){
        if(len==1){
            *buf=IIC_ReadByte(1);   //读数据,发送nACK
        }else{
            *buf=IIC_ReadByte(0);   //读数据,发送ACK
        }
        len--;
        buf++;
    }
    IIC_Stop();                     //产生一个停止条件

    return 0;
}
/*********************************************************************************************************
* 函 数 名 : Mpu6050_WriteByte
* 功能说明 : IIC写一个字节
* 形    参 : reg：寄存器地址；data：写入的数据；
* 返 回 值 : 0：成功；其他：失败
* 备    注 : 无
*********************************************************************************************************/
unsigned char Mpu6050_WriteByte(unsigned char reg,unsigned char data)
{

    IIC_Start();
    IIC_Send_Byte((MPU_ADDR<<1) | 0);
    if(IIC_Wait_Ack()){
        IIC_Stop();
        return 1;
    }
    IIC_Send_Byte(reg);
    IIC_Wait_Ack();
    //rt_hw_us_delay(5);
    IIC_Send_Byte(data);
    if(IIC_Wait_Ack()){
        IIC_Stop();
        return 1;
    }
    IIC_Stop();

    return 0;
}
/*********************************************************************************************************
* 函 数 名 : Mpu6050_ReadByte
* 功能说明 : IIC读一个字节
* 形    参 : reg：寄存器地址
* 返 回 值 : 读到的数据
* 备    注 : 无
*********************************************************************************************************/
unsigned char Mpu6050_ReadByte(unsigned char reg)
{
    unsigned char res;

    IIC_Start();
    IIC_Send_Byte((MPU_ADDR<<1) | 0);   //发送器件地址+写命令
    IIC_Wait_Ack();                     //等待应答
    IIC_Send_Byte(reg);                 //写寄存器地址
    IIC_Wait_Ack();                     //等待应答
    IIC_Start();
    IIC_Send_Byte((MPU_ADDR<<1) | 1);   //发送器件地址+读命令
    IIC_Wait_Ack();                     //等待应答
    res=IIC_Read_Byte(1);                //读取数据,发送nACK
    IIC_Stop();                         //产生一个停止条件

    return res;
}




/*********************************************************************************************************
* 函 数 名 : Mpu6050_Init
* 功能说明 : 初始化MPU6050
* 形    参 : 无
* 返 回 值 : 0：成功；其他：错误代码
* 备    注 : 无
*********************************************************************************************************/
unsigned char Mpu6050_Init(void)
{
    unsigned char res;
    GPIO_InitTypeDef GPIO_InitStruct;

//    RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE);
//
//    GPIO_InitStructure.GPIO_Pin   = SDA | SCL;
//    GPIO_InitStructure.GPIO_Mode  = GPIO_Mode_OUT;
//    GPIO_InitStructure.GPIO_OType = GPIO_OType_OD;
//    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
//    GPIO_InitStructure.GPIO_PuPd  = GPIO_PuPd_NOPULL;
//    GPIO_Init(GPIOB, &GPIO_InitStructure);
//
//    GPIO_SetBits(GPIOB, GPIO_Pin_6 | GPIO_Pin_7);

   __HAL_RCC_GPIOD_CLK_ENABLE();
   __HAL_RCC_GPIOA_CLK_ENABLE();
   __HAL_RCC_GPIOC_CLK_ENABLE();
   /* Configure GPIO_InitStructure */
   GPIO_InitStruct.Pin = SDA | SCL;
   GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
   GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_OD;
   GPIO_InitStruct.Pull = GPIO_NOPULL;
   HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
   SCL_HIGH();
   SDA_HIGH();


    Mpu6050_WriteByte(MPU_PWR_MGMT1_REG, 0X80); //复位MPU6050
    rt_hw_us_delay(500);
    rt_hw_us_delay(500);
    Mpu6050_WriteByte(MPU_PWR_MGMT1_REG, 0X00); //唤醒MPU6050
    Mpu6050_SetGyroFsr(3);                      //陀螺仪传感器,±2000dps
    Mpu6050_SetAccelFsr(0);                     //加速度传感器,±2g
    Mpu6050_SetRate(50);                        //设置采样率50Hz
    Mpu6050_WriteByte(MPU_INT_EN_REG, 0X00);    //关闭所有中断
    Mpu6050_WriteByte(MPU_USER_CTRL_REG, 0X00); //I2C主模式关闭
    Mpu6050_WriteByte(MPU_FIFO_EN_REG, 0X00);   //关闭FIFO
    Mpu6050_WriteByte(MPU_INTBP_CFG_REG, 0X80); //INT引脚低电平有效
    res = Mpu6050_ReadByte(MPU_DEVICE_ID_REG);

    if(res == MPU_ADDR){                            //器件ID正确
        Mpu6050_WriteByte(MPU_PWR_MGMT1_REG, 0X01); //设置CLKSEL,PLL X轴为参考
        Mpu6050_WriteByte(MPU_PWR_MGMT2_REG, 0X00); //加速度与陀螺仪都工作
        Mpu6050_SetRate(50);                        //设置采样率为50Hz
    }else{
        return 1;
    }

    return 0;
}
/*********************************************************************************************************
* 函 数 名 : Mpu6050_SetGyroFsr
* 功能说明 : 设置MPU6050陀螺仪传感器满量程范围
* 形    参 : fsr：0，±250dps；1，±500dps；2，±1000dps；3，±2000dps
* 返 回 值 : 0：成功；其他：失败
* 备    注 : 无
*********************************************************************************************************/
unsigned char Mpu6050_SetGyroFsr(unsigned char fsr)
{
    return Mpu6050_WriteByte(MPU_GYRO_CFG_REG, fsr<<3);     //设置陀螺仪满量程范围
}
/*********************************************************************************************************
* 函 数 名 : Mpu6050_SetAccelFsr
* 功能说明 : 设置MPU6050加速度传感器满量程范围
* 形    参 : fsr：0，±2g；1，±4g；2，±8g；3，±16g
* 返 回 值 : 0：成功；其他：失败
* 备    注 : 无
*********************************************************************************************************/
unsigned char Mpu6050_SetAccelFsr(unsigned char fsr)
{
    return Mpu6050_WriteByte(MPU_ACCEL_CFG_REG, fsr<<3);    //设置加速度传感器满量程范围
}
/*********************************************************************************************************
* 函 数 名 : Mpu6050_SetLPF
* 功能说明 : 设置MPU6050的数字低通滤波器
* 形    参 : lpf：数字低通滤波频率(Hz)
* 返 回 值 : 0：成功；其他：失败
* 备    注 : 无
*********************************************************************************************************/
unsigned char Mpu6050_SetLPF(unsigned short lpf)
{
    unsigned char data = 0;

    if(lpf >= 188){
        data = 1;
    }else if(lpf >= 98){
        data = 2;
    }else if(lpf >= 42){
        data = 3;
    }else if(lpf >= 20){
        data = 4;
    }else if(lpf >= 10){
        data = 5;
    }else{
        data = 6;
    }

    return Mpu6050_WriteByte(MPU_CFG_REG, data);    //设置数字低通滤波器
}
/*********************************************************************************************************
* 函 数 名 : Mpu6050_SetRate
* 功能说明 : 设置MPU6050的采样率(假定Fs=1KHz)
* 形    参 : rat：4~1000(Hz)
* 返 回 值 : 0：成功；其他：失败
* 备    注 : 无
*********************************************************************************************************/
unsigned char Mpu6050_SetRate(unsigned short rate)
{
    unsigned char data;

    if(rate > 1000){
        rate = 1000;
    }

    if(rate < 4){
        rate = 4;
    }

    data = 1000/rate - 1;
    data = Mpu6050_WriteByte(MPU_SAMPLE_RATE_REG, data);    //设置数字低通滤波器

    return Mpu6050_SetLPF(rate/2);                              //自动设置LPF为采样率的一半
}
/*********************************************************************************************************
* 函 数 名 : Mpu6050_GetTemperature
* 功能说明 : 读取MPU6050温度值
* 形    参 : 无
* 返 回 值 : 温度值（扩大了100倍）
* 备    注 : 无
*********************************************************************************************************/
short Mpu6050_GetTemperature(void)
{
    unsigned char buf[2];
    short raw;
    float temp;

    Mpu6050_ReadLen(MPU_ADDR, MPU_TEMP_OUTH_REG, 2, buf);
    raw = ((unsigned short)buf[0]<<8) | buf[1];
    temp = 36.53 + ((double)raw)/340;

    return temp * 100;
}
/*********************************************************************************************************
* 函 数 名 : Mpu6050_GetGyroscope
* 功能说明 : 读取MPU6050陀螺仪值（原始值）
* 形    参 : gx、gy、gz：陀螺仪x、y、z轴的原始读数（带符号）
* 返 回 值 : 0：成功；其他：失败
* 备    注 : 无
*********************************************************************************************************/
unsigned char Mpu6050_GetGyroscope(short *gx, short *gy, short *gz)
{
    unsigned char buf[6], res;

    res = Mpu6050_ReadLen(MPU_ADDR, MPU_GYRO_XOUTH_REG, 6, buf);
    if(res == 0){
        *gx = ((unsigned short)buf[0]<<8) | buf[1];
        *gy = ((unsigned short)buf[2]<<8) | buf[3];
        *gz = ((unsigned short)buf[4]<<8) | buf[5];
    }

    return res;
}
/*********************************************************************************************************
* 函 数 名 : Mpu6050_GetAccelerometer
* 功能说明 : 读取MPU6050加速度值（原始值）
* 形    参 : ax、ay、az：加速度计x、y、z轴的原始读数（带符号）
* 返 回 值 : 0：成功；其他：失败
* 备    注 : 无
*********************************************************************************************************/
unsigned char Mpu6050_GetAccelerometer(short *ax,short *ay,short *az)
{
    unsigned char buf[6], res;

    res = Mpu6050_ReadLen(MPU_ADDR,MPU_ACCEL_XOUTH_REG, 6, buf);
    if(res == 0){
        *ax = ((unsigned short)buf[0]<<8) | buf[1];
        *ay = ((unsigned short)buf[2]<<8) | buf[3];
        *az = ((unsigned short)buf[4]<<8) | buf[5];
    }

    return res;
}


